#include <ros/ros.h>
#include <ros/serialization.h>

#include "cg_interfaces/BridgeControl.h"
#include "cg_interfaces/BridgeSensor.h"
#include "cg_interfaces/ChassisStruct.h"

#include "cg_remote_control/udp_backend.hpp"

using namespace CG;

UDPBackEnd *udpBackend;
NetEndPoint target_point;
const int buffer_size = 1024;
const int hz = 50;
const int depth = 5;
unsigned char serialize_buffer[buffer_size];
unsigned char rec_buffer[buffer_size];

enum class MsgType : unsigned char
{
    Undefine = 0,
    BridgeControl = 1,
    BridgeSensor = 2,
    ChassisStruct = 3
};

void MasterControlCallback(const cg_interfaces::BridgeControl::ConstPtr &msg)
{
    // ROS_INFO("virtual_bridge: controlCallback called");
    serialize_buffer[0] = static_cast<unsigned char>(MsgType::BridgeControl);
    int length = ros::serialization::serializationLength(*msg);
    ros::serialization::OStream ostream(serialize_buffer + 1, length);
    ros::serialization::serialize(ostream, *msg);
    udpBackend->Send(serialize_buffer, length + 1, target_point);
}

void MasterStructCallback(const cg_interfaces::ChassisStruct::ConstPtr &msg)
{
    // ROS_INFO("virtual_bridge: structCallback called");
    serialize_buffer[0] = static_cast<unsigned char>(MsgType::ChassisStruct);
    int length = ros::serialization::serializationLength(*msg);
    ros::serialization::OStream ostream(serialize_buffer + 1, length);
    ros::serialization::serialize(ostream, *msg);
    udpBackend->Send(serialize_buffer, length + 1, target_point);
}

void SlaverSensorCallback(const cg_interfaces::BridgeSensor::ConstPtr &msg)
{
    // ROS_INFO("virtual_bridge: sensorCallback called");
    serialize_buffer[0] = static_cast<unsigned char>(MsgType::BridgeSensor);
    int length = ros::serialization::serializationLength(*msg);
    ros::serialization::OStream ostream(serialize_buffer + 1, length);
    ros::serialization::serialize(ostream, *msg);
    udpBackend->Send(serialize_buffer, length + 1, target_point);
}

bool TryLoadParam(int &id, int &hostport, std::string &ip, int &port, bool &mode_master)
{
    ros::NodeHandle n("~");
    if (!n.getParam("id", id))
    {
        ROS_ERROR("virtual_bridge node: id not found");
        return false;
    }
    if (!n.getParam("host_port", hostport))
    {
        ROS_ERROR("virtual_bridge node: hostport not found");
        return false;
    }
    if (!n.getParam("ip", ip))
    {
        ROS_ERROR("virtual_bridge node: ip not found");
        return false;
    }
    if (!n.getParam("port", port))
    {
        ROS_ERROR("virtual_bridge node: port not found");
        return false;
    }
    std::string mode;
    if (!n.getParam("mode", mode))
    {
        ROS_ERROR("virtual_bridge node: mode_master not found");
        return false;
    }
    if (mode == "slave")
    {
        mode_master = false;
    }
    else if (mode == "master")
    {
        mode_master = true;
    }
    else
    {
        ROS_ERROR("virtual_bridge node: mode_master should be either slave or master");
        return false;
    }
    return true;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "virtual_bridge");
    ROS_INFO("virtual_bridge node started");

    std::string ip;
    int id, port, hostport;
    bool mode_master;
    if (!TryLoadParam(id, hostport, ip, port, mode_master))
    {
        ROS_ERROR("virtual_bridge node: failed to load parameters");
        ros::shutdown();
        return -1;
    }
    ROS_INFO("virtual_bridge node: id: %d, ip: %s, port: %d, mode_master: %d", id, ip.c_str(), port, mode_master);

    target_point = NetEndPoint(ip, port);
    udpBackend = new UDPBackEnd(NetEndPoint(hostport));
    ssize_t rec_length = 0;
    NetEndPoint remote_endpoint;

    ros::NodeHandle n;
    std::string topic_root = "bridge" + std::to_string(id) + '/';
    if (mode_master)
    {
        // chassis -> bridge -> slaver :control,struct
        // slaver -> bridge -> chassis :sensor
        auto controlSub = n.subscribe<cg_interfaces::BridgeControl>(topic_root + "control", depth, MasterControlCallback);
        auto sensorPub = n.advertise<cg_interfaces::BridgeSensor>(topic_root + "sensor", depth);
        auto structSub = n.subscribe<cg_interfaces::ChassisStruct>(topic_root + "struct", depth, MasterStructCallback);

        ros::Rate rate(hz);
        while (ros::ok())
        {
            rate.sleep();
            while (udpBackend->Receive(rec_buffer, buffer_size, rec_length, remote_endpoint))
            {
                MsgType msg_type = static_cast<MsgType>(rec_buffer[0]);
                if (msg_type == MsgType::BridgeSensor)
                {
                    cg_interfaces::BridgeSensor msg;
                    ros::serialization::IStream istr(rec_buffer + 1, rec_length - 1);
                    ros::serialization::deserialize(istr, msg);
                    sensorPub.publish(msg);
                }
                else
                {
                    ROS_WARN("virtual_bridge node: unknown message type");
                }
            }
            ROS_INFO("empty receive buffer");
            ros::spinOnce();
        }
    }
    else
    {
        // chassis -> bridge -> master :sensor
        // master -> bridge -> chassis :control,struct
        auto controlPub = n.advertise<cg_interfaces::BridgeControl>(topic_root + "control", depth);
        auto sensorSub = n.subscribe<cg_interfaces::BridgeSensor>(topic_root + "sensor", depth, SlaverSensorCallback);
        auto structPub = n.advertise<cg_interfaces::ChassisStruct>(topic_root + "struct", depth);

        ros::Rate rate(hz);
        while (ros::ok())
        {
            rate.sleep();
            while (udpBackend->Receive(rec_buffer, buffer_size, rec_length, remote_endpoint))
            {
                MsgType msg_type = static_cast<MsgType>(rec_buffer[0]);
                if (msg_type == MsgType::BridgeControl)
                {
                    cg_interfaces::BridgeControl msg;
                    ros::serialization::IStream istr(rec_buffer + 1, rec_length - 1);
                    ros::serialization::deserialize(istr, msg);
                    controlPub.publish(msg);
                }
                else if (msg_type == MsgType::ChassisStruct)
                {
                    cg_interfaces::ChassisStruct msg;
                    ros::serialization::IStream istr(rec_buffer + 1, rec_length - 1);
                    ros::serialization::deserialize(istr, msg);
                    structPub.publish(msg);
                }
                else
                {
                    ROS_WARN("virtual_bridge node: unknown message type");
                }
            }
            ROS_INFO("empty receive buffer");
            ros::spinOnce();
        }
    }

    ros::shutdown();
    return 0;
}